# 研究机构：上海大学机自学院
# 研 究 生：王强
# 开发时间：2024/6/4 21:01
import numpy as np


class TrackInitiation:
    def __init__(self, threshold, M, N):
        self.starting_tracks = []                                        # 雷达起始成功航迹存储，存储格式{Id: 批号, points: 起始航迹}
        self.track_id = 0                                       # 航迹起始成功批号
        self.threshold = threshold                              # 航迹关联最小阈值
        self.min_scans = M                                      # 达到航迹起始的的最小雷达扫描次数
        self.min_batch = N                                      # 雷达航机起始成功最小点迹个数，例如雷达扫描五次，其中一组起始航迹中点迹数量最少不低于3

    # 求解两点间的距离，并于门限阈值做比较
    def distance(self, point1, point2):
        return np.linalg.norm(point1 - point2)

    # 雷达航机起始关联规则，输入相邻两个时刻的点云数据，得到关联成功的航迹索引值
    def associate_points(self, points1, points2):
        associations = []                                                                   # 存储前后时刻满足关联规则的点云数据索引值
        seen_i = set()                                                                      # 存储前后时刻满足关联规则的点云数据索引的x值
        seen_j = set()                                                                      # 存储前后时刻满足关联规则的点云数据索引的y值
        # 遍历前后相邻两个时刻的点云数据组，寻找满足关联规则的点云
        for i, point1 in enumerate(points1):
            for j, point2 in enumerate(points2):
                if self.distance(point1, point2) < self.threshold:
                    # 避免前后相邻两个时刻的点云数据组，出现一个点云关联多个点云情况
                    if i not in seen_i and j not in seen_j:
                        associations.append((i, j))
                        seen_i.add(i)
                        seen_j.add(j)
        # if associations ==[]:
        #     associations.append((100, 100))
        return associations

    # 雷达航机起始关联，将雷达扫面M次周期内的点云数据关联起来，关联数量大于N则其实成功
    def start_tracks(self, scans):
        assert len(scans) == self.min_scans                                       # 雷达扫描应正好包含5个时间步长
        seen_t = []                                                               # 存储关联矩阵为空的雷达批次的索引值
        tracks = {}                                                               # 中间变量用于存储雷达航迹起始成功的点云数据的索引值
        for t in range(self.min_scans-1):                                         # 遍历数据关联索引，寻找对应点云数据
            associations = self.associate_points(scans[t], scans[t + 1])          # 返回时刻前后数据关联成功的索引
            # print('associations', associations)
            # 航机起始，将5次扫描的量测依据前后时候关联起来
            # 将字典中的键值对颠倒，其中值仅保留值中的最后一个数值，便于与下一组索引关联，将每一组所串联为一条完整的雷达航迹
            if associations == [] and t > 0:                                                 # 避免跳跃时刻关联出现雷达点云索引值对应错误
                seen_t.append(t)
            rever_tracks = {value[-1]: key for key, value in tracks.items()}
            # print('rever_tracks', rever_tracks)
            # 变量，实现M次扫描周期内的雷达航迹起始，这里只是雷达索引值对应每一航迹串联
            for i, j in associations:
                if i not in rever_tracks:
                    tracks[t, i] = []
                    tracks[t, i].append(j)
                else:
                    tracks[rever_tracks[i]].append(j)
            # print('tracks.get', {value[-1] for key, value in tracks.items()})
            # 避免航迹起始过程中跳跃关联
            # for key, value in list(tracks.items()):
            #     if t - len(value) == 1:
            #         del tracks[key]
        # print(tracks)

        # 将上述雷达航迹起始的索引值找到对应点迹生成带有批号的雷达航迹起始
        initiated_tracks = []                                                     # 中间变量用于存储雷达航迹起始成功的点云数据，不带批号
        # uninitiated_tracks = scans                                                # 中间变量用于存储未航迹起始成功的点云数据，不带批号
        # print('tracks.items()', tracks.items())
        for track, indices in tracks.items():
            if len(indices) >= self.min_batch:
                # print(indices)
                track_points = [scans[track[0]][track[1]]]                       # 存储成功航机起始的第一个点云数据
                for t, index in enumerate(indices):
                    if t not in seen_t:
                        # print(len(scans[t + (1 + track[0])]))
                        # print(t, track[0], t + 1 + track[0],'scans[t + (1 + track[0])]',scans[t + (1 + track[0])])
                        track_points.append(scans[t + 1 + track[0]][index])
                    # else:
                    #     print('seen_t',seen_t)
                self.starting_tracks.append({
                    'id': self.track_id,
                    'points': track_points
                })
                self.track_id += 1
                initiated_tracks.append(track_points)

        # 将上述雷达M次扫描结果中删除，已经航迹起始成功的点迹
        for t in range(len(initiated_tracks)):
            for track_point in initiated_tracks[t]:
                # print(track_point)
                for i, scan in enumerate(scans):
                    indices_to_delete = [j for j, row in enumerate(scan) if np.all(row == track_point)]
                    # print(indices_to_delete)
                    scans[i] = np.delete(scan, indices_to_delete, axis=0)

        return initiated_tracks

# x下面为基于MN逻辑法的航迹起始测试模块
if __name__ == '__main__':
# 测试数据（假设每个扫描有5个点，每个点有x, y坐标）
    scansssss = [
        np.array([[1.4, 2.4], [3.8, 4.9], [3.4, 4.5], [10.4, 5.3], [5.4, 6.4]]) * 10,
        np.array([[1.3, 20.3], [3.3, 4.4], [3.3, 4.3], [6, 6], [4.3, 5.3], [5.3, 6.3]]) * 10,
        np.array([[1.2, 10.2], [2.8, 3.8], [4.2, 5.2], [7, 7], [3.2, 4.2], [5.2, 6.2]]) * 10,
        np.array([[1.1, 2.1], [2.5, 3.5], [3.1, 4.1], [4.1, 5.1], [7, 7], [5.1, 6.1]]) * 10,
        np.array([[1, 2], [2, 3], [3, 4], [4, 5], [10, 10], [7, 7], [5, 6]])*10,
        np.array([[1.4, 2.4], [3.8, 4.9], [3.4, 4.5], [10.4, 5.3], [5.4, 6.4]]) * 10,
        np.array([[1.3, 20.3], [3.3, 4.4], [3.3, 4.3], [6, 6], [4.3, 5.3], [5.3, 6.3]]) * 10,
        np.array([[1.2, 10.2], [2.8, 3.8], [4.2, 5.2], [7, 7], [3.2, 4.2], [5.2, 6.2]]) * 10,
        np.array([[1.1, 2.1], [2.5, 3.5], [3.1, 4.1], [4.1, 5.1], [7, 7], [5.1, 6.1]]) * 10,
        np.array([[1, 2], [2, 3], [3, 4], [4, 5], [10, 10], [7, 7], [5, 6]]) * 10,
        np.array([[1.4, 2.4], [3.8, 4.9], [3.4, 4.5], [10.4, 5.3], [5.4, 6.4]]) * 10,
        np.array([[1.3, 20.3], [3.3, 4.4], [3.3, 4.3], [6, 6], [4.3, 5.3], [5.3, 6.3]]) * 10,
        np.array([[1.2, 10.2], [2.8, 10.8], [4.2, 5.2], [7, 7], [3.2, 4.2], [5.2, 6.2]]) * 10,
        np.array([[1.1, 2.1], [2.5, 3.5], [3.1, 4.1], [4.1, 5.1], [7, 7], [5.1, 6.1]]) * 10,
        np.array([[1, 2], [2, 3], [3, 4], [4, 5], [10, 10], [7, 7], [5, 6]]) * 10,
        np.array([[1.4, 2.4], [3.8, 4.9], [10.4, 4.5], [10.4, 5.3], [5.4, 6.4]]) * 10,
        np.array([[1.3, 20.3], [3.3, 4.4], [3.3, 4.3], [6, 6], [4.3, 5.3], [5.3, 6.3]]) * 10,
        np.array([[1.2, 10.2], [2.8, 3.8], [4.2, 5.2], [7, 7], [3.2, 4.2], [5.2, 6.2]]) * 10,
        np.array([[1.1, 2.1], [2.5, 3.5], [3.1, 4.1], [4.1, 5.1], [7, 7], [5.1, 6.1]]) * 10,
        np.array([[1, 2], [2, 3], [3, 4], [4, 5], [10, 10], [7, 7], [5, 6]]) * 10,
    ]

    M = 5
    N = 3
    scans = []
    t = len(scansssss)-1
    for step in range(t):
        scans.append(np.array(scansssss[step]))
        # 初始化算法
        print(len(scans))
        # print('scans 1', scans)
        print('step', step)
        if len(scans) == M:
            tracker = TrackInitiation(threshold=5, M=M, N=N)
            initiated_tracks = tracker.start_tracks(scans)
            # print('scans 2', scans)
            scans.pop(0)
            # print('scans 3', scans)
            print(tracker.starting_tracks)
        # # 打印结果
        #     for track in initiated_tracks:
        #         print("Track initiated:", track)

